#include <unistd.h>
#include <stdio.h>
#include "serial_interface_linux.h"
#include "witdatapkg.h"

int main(int argc, char const *argv[]) {
	SerialInterfaceLinux* serial = new SerialInterfaceLinux();

	wit_private_msg::WitDataPkg* wtgahrs1pkg = new wit_private_msg::WitDataPkg(); 
	
	serial->SetReadCallback(std::bind(&wit_private_msg::WitDataPkg::RecvDataCallback, 
		wtgahrs1pkg, std::placeholders::_1, std::placeholders::_2));

	std::string port_name = "/dev/ttyUSB0";
	uint32_t com_baudrate = 9600;
	if (!serial->Open(port_name, com_baudrate)) {
		perror("open error\n");
		return -1;
	}

  while (1) {

		if (wtgahrs1pkg->IsImuDataReady()) {
			wit_private_msg::WitImuAccelerometerType acc = wtgahrs1pkg->GetImuAccelerometerData();
			wit_private_msg::WitImuAngularvelType angularvel = wtgahrs1pkg->GetImuAngularvelData();
			wit_private_msg::WitImuAngularType angular = wtgahrs1pkg->GetImuAngularData();
			printf("acc_x: %f (g), acc_y: %f (g), acc_z: %f (g); angle_vel_x: %f (deg/s), angle_vel_y: %f (deg/s), angle_vel_z: %f (deg/s); roll-x: %f (deg), pitch-y: %f (deg), yaw-z: %f (deg)\n",
			 acc.acc_x,
			 acc.acc_y,
			 acc.acc_z,
			 angularvel.angularvel_x,
			 angularvel.angularvel_y,
			 angularvel.angularvel_z,
			 angular.roll_x,
			 angular.pitch_y,
			 angular.yaw_z);
			printf("-----------------------------------------\n");
		}

		if (wtgahrs1pkg->IsGnssDataReady()) {
			wit_private_msg::WitGnssLocationType gnssdata = wtgahrs1pkg->GetGnssLocationData();
			printf("lng: %f (deg), lat: %f (deg), speed: %d (0.1m/s), altitude: %d (m)\n", 
						gnssdata.current_pos_lng, 
						gnssdata.current_pos_lat, 
						gnssdata.actual_speed, 
						gnssdata.actual_altitude);
			printf("-----------------------------------------\n");
		}

		usleep(100 * 1000);
  }

  serial->Close();

  return 0;
}
